#ifndef MATHFUNCTIONS_H
#define MATHFUNCTIONS_H

#if defined __cplusplus
extern "C" {
#endif

#include <assert.h>
#include <math.h>
#include <float.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>
#include <unistd.h>
#include <sys/time.h>


typedef struct
{
  double x;
  double y;
} vector2_t;

#define VECTOR2

typedef struct
{
  vector2_t pos;
  double rot;
} pose2_t;

#define POSE2

/** @brief 4 by 4 matrix structure. */
typedef struct
{
  double m[4][4];
} matrix44_t;

#define MATRIX44

/** @brief 4 vector structure. */
typedef struct
{
  double x;
  double y;
  double z;
  double w;
} vector4_t;

#define VECTOR4

// @brief Add two poses c = a + b 
vector2_t pose2_add_pos(vector2_t a, pose2_t b);
vector2_t pose2_sub_pos(vector2_t a, pose2_t b);


// @brief Subtract poses: c = a - b. 
pose2_t pose2_sub(pose2_t a, pose2_t b);
pose2_t pose2_add(pose2_t a, pose2_t b);

double pose2_add_rot(double a, pose2_t b);
double pose2_sub_rot(double a, pose2_t b);

  
  #if defined __cplusplus
  }
#endif
#endif
